#include "apf.h"

using namespace NS_APF;

Vec2 node::attractive_force_calculate(Vec2 _object_position, float _k_a,
                                      float _goal_max_distance) {
  std::cout << "the goal is " << _object_position << "  ";
  std::cout << "current position is " << this->position_ << " ";
  float distance = pow(_object_position.x - this->position_.x, 2) +
                   pow(_object_position.y - this->position_.y, 2);

  float theta = atan((_object_position.y - this->position_.y) /
                     (_object_position.x - this->position_.x));
  float combined_force = _k_a * distance;
  //        (distance > _goal_max_distance ? _goal_max_distance : distance);

  Vec2 attractive_force_(combined_force * cos(theta),
                         combined_force * sin(theta));

  return attractive_force_;
}

Vec2 node::spring_force_calculate(Vec2 _object_position, float _k_s) {
  float distance = pow(_object_position.x - this->position_.x, 2) +
                   pow(_object_position.y - this->position_.y, 2);

  float theta = atan((_object_position.y - this->position_.y) /
                     (_object_position.x - this->position_.x));

  float combined_force = _k_s * exp(-distance);

  Vec2 attractive_force_(combined_force * cos(theta),
                         combined_force * sin(theta));

  return attractive_force_;
}

Vec2 node::revolution_force_calculate(Vec2 _object_position, float _k_r,
                                      float _detect_radius) {
  float distance = pow(_object_position.x - this->position_.x, 2) +
                   pow(_object_position.y - this->position_.y, 2);

  if (distance > _detect_radius) {
    return Vec2(0, 0);
  } else {
    float tmp = (1 / distance - 1 / _detect_radius) / pow(distance, 3);
    return Vec2(tmp * (this->position_.x - _object_position.x),
                tmp * (this->position_.y - _object_position.y));
  }
}

APF::APF(APF_Param *_param_p, Vec2 _goal, int _fol_num) {
  fol_num_ = _fol_num;

  param_.k_a_ = _param_p->k_a_;
  param_.k_r_ = _param_p->k_r_;
  param_.k_s_ = _param_p->k_s_;
  param_.goal_max_distance_ = _param_p->goal_max_distance_;
  param_.detect_radius_ = _param_p->detect_radius_;
  param_.UAV_virtual_mass = _param_p->UAV_virtual_mass;
  param_.init_UAV_pos_ = _param_p->init_UAV_pos_;
  param_.obstacle_position_ = _param_p->obstacle_position_;
  goal_ = _goal;
  std::vector<node> fol_node(fol_num_);

  UAV_List_.insert(UAV_List_.end(), fol_node.begin(), fol_node.end());

  std::vector<std::vector<float>> adjacent_tmp = {
      {0, 1, 0, 0, 0, 1}, {1, 0, 1, 0, 0, 0}, {0, 1, 0, 1, 0, 1},
      {1, 0, 1, 0, 1, 0}, {0, 1, 0, 1, 0, 1}, {1, 0, 1, 0, 1, 0},
  };
  adjacent_ = adjacent_tmp;

  for (int i = 0; i < _fol_num; i++) {
    UAV_List_[i].position_ = param_.init_UAV_pos_[i];
  }
}

void APF::UAV_combine_foce_calculate(int _UAV_index) {
  UAV_List_[_UAV_index].revolution_force_.clear();
  for (auto obstack : param_.obstacle_position_) {
    UAV_List_[_UAV_index].revolution_force_ =
        UAV_List_[_UAV_index].revolution_force_ +
        UAV_List_[_UAV_index].revolution_force_calculate(obstack, param_.k_r_,
                                                         param_.detect_radius_);
  }

  UAV_List_[_UAV_index].spring_force_.clear();
  for (int index = 0; index < fol_num_ + 1; index++) {
    if (adjacent_[_UAV_index][index] == 0)
      continue;

    UAV_List_[_UAV_index].spring_force_ =
        UAV_List_[_UAV_index].spring_force_ +
        UAV_List_[_UAV_index].spring_force_calculate(UAV_List_[index].position_,
                                                     param_.k_s_);
  }

  UAV_List_[_UAV_index].attractive_force_.clear();
  UAV_List_[_UAV_index].attractive_force_ =
      UAV_List_[_UAV_index].attractive_force_calculate(
          goal_, param_.k_a_, param_.goal_max_distance_);

  UAV_List_[_UAV_index].sum_force_ = UAV_List_[_UAV_index].attractive_force_ -
                                     UAV_List_[_UAV_index].revolution_force_ -
                                     UAV_List_[_UAV_index].spring_force_;
}

Vec2 APF::UAV_velocity_calculate(int _UAV_index, Vec2 _current_position) {
  UAV_List_[_UAV_index].position_update(_current_position);

  UAV_combine_foce_calculate(_UAV_index);

  UAV_List_[_UAV_index].velocity_ =
      UAV_List_[_UAV_index].velocity_ +
      UAV_List_[_UAV_index].sum_force_ / param_.UAV_virtual_mass;

  return UAV_List_[_UAV_index].velocity_;
}

std::vector<Vec2> APF::calculate(std::vector<Vec2> _current_position_list) {
  std::vector<Vec2> result(fol_num_ + 1);

  for (int index = 0; index < fol_num_ + 1; index++) {
    result[index] =
        UAV_velocity_calculate(index, _current_position_list[index]);
  }
  return result;
}
